/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 *																 *
 *	r2GL - r2 Graphs Library									 *
 *																 *
 *	File:		EuclideanGraph.cpp								 *
 *	Authors:	Rafael Kikuchi ( shinjikik@gmail.com )			 *
 *				Rafael Romeiro ( rafaelromeiro@gmail.com )		 *
 *	Date:		29/11/2008										 *
 *	License:	CC-GNU LGPL										 *
 *				http://creativecommons.org/licenses/LGPL/2.1/	 *
 *	Version:	0.1.0											 *
 *																 *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include "EuclideanGraph.h"

namespace r2GL
{

EuclideanGraph::EuclideanGraph (const char *FileInput)
{
	structureType = r2MATRIX;
	dirtyComponents = true;
	isWeighted = true;
	hasNegative = false;
	isDirected = r2UNDIRECTED;

	string line;
	int lineNum;
	ifstream input(FileInput);
	
	lineNum = 0;
	while (!input.eof())
	{
		float args[2], numArgs = 0;

		getline(input, line);
		if (line[0] != '#' && line.size() > 0) // Ignore lines starting with "#"
		{
			istringstream ss(line);
			for (int i=0; ss >> args[i]; i++)
				numArgs++;

			lineNum++;
		}
		else
			continue;

		if (lineNum == 1) // Number of nodes
		{
			numNodes = args[0];
			numEdges = numNodes*(numNodes-1)/2;
			nodes.resize(numNodes);

			for (int i=0; i < numNodes; i++)
				nodes[i] = EuclideanNode(i+1);

			WeightMatrix.resize(numNodes);
			for (int j = 0; j < numNodes ; j++)
				WeightMatrix[j].resize(numNodes);

			AllocateInMemory();

			for (int i=0; i < numNodes; i++)
				for (int j=0; j < numNodes; j++)
					adjMatrix[i][j] = 1;
		}
		else
		{
			nodes[lineNum-2].X(args[0]);
			nodes[lineNum-2].Y(args[1]);
		}
	}
	input.close();

	for (int i=0; i < numNodes; i++)
	{
		for (int j=i; j < numNodes; j++)
		{
			WeightMatrix[i][j] = sqrt((nodes[i].X()-nodes[j].X())*(nodes[i].X()-nodes[j].X()) +
									(nodes[i].Y()-nodes[j].Y())*(nodes[i].Y()-nodes[j].Y()));

			WeightMatrix[j][i] = WeightMatrix[i][j];
		}
	}
}

void EuclideanGraph::WriteTSP (const char *FileOutput)
{
	ofstream output(FileOutput);
	if (output.is_open())
	{
		list<r2GL::EuclideanNode*>::iterator it;
		it = path.begin();
		output << (*it)->GetID();
		it++;
		for ( ; it != path.end() ; it++ )
			output << "\n" << (*it)->GetID();

		output.close();
	}
}

void EuclideanGraph::TSP (void)
{
	path.clear();

	int upNode = 0, downNode = 0, leftNode = 0, rightNode = 0;
	float comX, comY; // COM = Center of Mass
	float maxSqDist, newSqDist, minDeltaDist, newDeltaDist;
	int nextNode, A, B;
	list<r2GL::EuclideanNode*>::iterator bestOpt;
	list<r2GL::EuclideanNode*>::iterator it;

	for ( int i = 0 ; i < nodes.size() ; i++ )
	{
		nodes[i].SetMark( 0 );
		if ( nodes[upNode].Y() < nodes[i].Y() )
			upNode = i;
		if ( nodes[downNode].Y() > nodes[i].Y() )
			downNode = i;
		if ( nodes[rightNode].X() < nodes[i].X() )
			rightNode = i;
		if ( nodes[leftNode].X() > nodes[i].X() )
			leftNode = i;
	}

	path.push_back(&nodes[upNode]);
	nodes[upNode].SetMark( 1 );
	if ( nodes[rightNode].GetMark() == 0 )
	{
		path.push_back(&nodes[rightNode]);
		nodes[rightNode].SetMark( 1 );
	}
	if ( nodes[downNode].GetMark() == 0 )
	{
		path.push_back(&nodes[downNode]);
		nodes[downNode].SetMark( 1 );
	}
	if ( nodes[leftNode].GetMark() == 0 )
	{
		path.push_back(&nodes[leftNode]);
		nodes[leftNode].SetMark( 1 );
	}
	path.push_back(&nodes[upNode]);

	//WriteTSP("path_0.txt");
	//int h = 1;

	while ( path.size() < (nodes.size()+1) )
	{
		comX = 0;
		comY = 0;
		it = path.begin();
		it++;
		for ( ; it != path.end() ; it++ )
		{
			comX += (*it)->X();
			comY += (*it)->Y();
		}
		comX = comX / (path.size()-1);
		comY = comY / (path.size()-1);

		maxSqDist = 0;
		for ( int i = 0 ; i < nodes.size() ; i++ )
		{
			if ( nodes[i].GetMark() == 0 )
			{
				newSqDist = (nodes[i].X()-comX)*(nodes[i].X()-comX)+(nodes[i].Y()-comY)*(nodes[i].Y()-comY);
				if ( maxSqDist <= newSqDist )
				{
					maxSqDist = newSqDist;
					nextNode = i;
				}
			}
		}

		it = path.begin();
		A = (*it)->GetID() - 1;
		it++;
		B = (*it)->GetID() - 1;
		minDeltaDist = WeightMatrix[A][nextNode] + WeightMatrix[B][nextNode] - WeightMatrix[A][B];
		bestOpt = it;
		it++;
		for ( ; it != path.end() ; it++ )
		{
			A = B;
			B = (*it)->GetID() - 1;
			newDeltaDist = WeightMatrix[A][nextNode] + WeightMatrix[B][nextNode] - WeightMatrix[A][B];
			if ( minDeltaDist > newDeltaDist )
			{
				minDeltaDist = newDeltaDist;
				bestOpt = it;
			}
		}

		path.insert( bestOpt, &nodes[nextNode] );
		nodes[nextNode].SetMark( 1 );

		/*ostringstream o;
		o << h;
		WriteTSP(( "path_" + o.str() + ".txt" ).c_str());
		h++;*/
	}
}

float EuclideanGraph::GetTSPTotalCost (void)
{
	float TotalCost = 0;
	int A, B;
	list<r2GL::EuclideanNode*>::iterator it;
	it = path.begin();
	B = (*it)->GetID() - 1;
	it++;
	for ( ; it != path.end() ; it++ )
	{
		A = B;
		B = (*it)->GetID() - 1;
		TotalCost += WeightMatrix[A][B];
	}

	return TotalCost;
}

} // namespace
